#include "CODSensor.h"
#define DevAddr 0x02 //设备ID,应设置为
static unsigned char _SendData[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
static unsigned char _RecvData[7] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};

bool CODSensor::initCODSensor(Modbus_RTU * _modbus)
{
    DevID = DevAddr & 0xFF;//设备ID
    modbus = _modbus;
    short id = ReadFun(DevID_REG);
    if ((id & 0xFF) == DevAddr)
        return true;
    else return false;
}

short CODSensor::ReadFun(CODSensor::FuncCode func)
{
    _SendData[0] = (DevID & 0xFF);//设备地址
    _SendData[1] = (CODSensor::ReadFuncCode & 0xFF); //读取
    _SendData[2] = ((func>>8) & 0xFF);//待读取的起始地址高八位
    _SendData[3] = (func & 0xFF);//待读取的起始地址高八位
    _SendData[4] = (0x00); //读取的数量
    _SendData[5] = 0x01; //读取的数量低八位
    uint16_t crc = this->modbus->CRC16_MudBus(_SendData, 6);//进行CRC校验
    _SendData[6] = (crc & 0xFF);//CRC的低八位
    _SendData[7] = ((crc>>8) & 0xFF);//CRC的高八位
    this->modbus->SendData(_SendData, 8);//发送数据
    delay(200);//等待100ms返回数据
    bool ret = this->modbus->RecvData(_RecvData, 7);//接收数据
    if (ret == false)
    {
        return 0xFFFF;
    }
    uint16_t crc_Recv = this->modbus->CRC16_MudBus(_RecvData, 5);//进行CRC校验
    if (((crc_Recv>>8) & 0xFF) != _RecvData[6] || (crc_Recv & 0xFF) != _RecvData[5])//校验成功
    {
        Serial.println("ReadFun : CRC Check failed!!!");
        return 0x0000;
    }
    return ((_RecvData[3]<<8) | _RecvData[4]);
}

bool CODSensor::SetFunc(CODSensor::FuncCode func, short SetData)
{
    _SendData[0] = (DevID & 0xFF);//设备地址
    _SendData[1] = (CODSensor::SetFuncCOde & 0xFF); //读取
    _SendData[2] = ((func>>8) & 0xFF);//待读取的起始地址高八位
    _SendData[3] = (func & 0xFF);
    _SendData[4] = ((SetData>>8) & 0xFF);
    _SendData[5] = (SetData & 0xFF);
    uint16_t crc = this->modbus->CRC16_MudBus(_SendData, 6);//进行CRC校验
    _SendData[6] = (crc & 0xFF);//CRC的低八位
    _SendData[7] = ((crc>>8) & 0xFF);//CRC的高八位
    this->modbus->SendData(_SendData, 8);//发送数据
    delay(200);//等待100ms返回数据
    bool ret = this->modbus->RecvData(_SendData, 8);//接收数据
    if (ret == false)
    {
        return false;
    }
    uint16_t crc_Recv = this->modbus->CRC16_MudBus(_SendData, 6);//进行CRC校验
    if (((crc_Recv>>8) & 0xFF) != _SendData[7] || (crc_Recv & 0xFF) != _SendData[6])//校验失败
    {
        Serial.println("SetFunc : RecvData CRC Check failed!!!");
        return false;
    }
    return true;
}


float CODSensor::ReadPH(void)
{
    short Raw = ReadFun(ReadPH_REG);
    if (Raw == 0xFFFF) // 说明返回数据是错的
    {
        return 0.0;
    }
    return Raw / 100.0;
}

float CODSensor::ReadTemperature(void)
{
    short Raw = ReadFun(ReadTemperature_REG);
    if (Raw == 0xFFFF) // 说明返回数据是错的
    {
        return 0.0;
    }
    return Raw / 10.0;
}

float CODSensor::ReadCOD(void)
{
    short Raw = ReadFun(ReadCOD_REG);
    if (Raw == 0xFFFF) // 说明返回数据是错的
    {
        return 0.0;
    }
    return Raw / 100.0;
}